
//#include <iostream>
#include <iostream>
#include "ros/ros.h"
#include <tf/transform_broadcaster.h>
#include "SerialStream.h"

/**
* 解析buf的内容并计算生成位置信息。
*/
bool compute_pos(char buf_0[], char buf_1[], char buf_2[], int &x, int &y)
{
    static int i=0;

    x = 0.2*i;
    y = 0.3*i;
    i++;
    return true;
}

/**
* 从串口读取指定长度的数据。由于一些未知原因，无法使用read函数直接读取指定长度的数据，所以有了这个函数。
*/
void ReadSerial( LibSerial::SerialStream &il_serial, char buf[], int len)
{
    for(int i=0; i<len; i++){
        il_serial.read( buf+i, 1);
    }
}


/**
* Init serial port for indor-localization model.
*/
void InitSerialPort(LibSerial::SerialStream &il_serial)
{
   /**
    * The various available baud rates are defined in SerialStreamBuf 
    * class. This is to be changed soon. All serial port settings will 
    * be placed in in the SerialPort class.
    */
    il_serial.SetBaudRate( LibSerial::SerialStreamBuf::BAUD_115200 ) ;
    
   /**
    * Use 8 bit wide characters.
    */
    il_serial.SetCharSize( LibSerial::SerialStreamBuf::CHAR_SIZE_8 ) ;
    
   /**
    * Use one stop bit.
    */
    il_serial.SetNumOfStopBits(1) ;   
    
   /**
    * Use odd parity during serial communication.
    */
    il_serial.SetParity( LibSerial::SerialStreamBuf::PARITY_NONE ) ;

   /**
    * Use hardware flow-control.
    */
    il_serial.SetFlowControl( LibSerial::SerialStreamBuf::FLOW_CONTROL_NONE ) ;
    
}


/**
* 从室内定位模块的串口读取数据，并存入三个buf中。
*/
bool ReadPosSensor(LibSerial::SerialStream &il_serial , char buf_0[11]
    , char buf_1[11], char buf_2[11], int len=11)
{
    // Find Message Header, it is "0xeo 0x00"
    while(1){
        char data[2];
        il_serial.read( data, 1);
        if(data[0]==-32){
            il_serial.read( data+1, 1);
            if(data[1]==0x0){
                buf_0[0]=0xe0;
                buf_0[1]=0x00;
                break;
            }
        }
    }

    // Read message body.
    ReadSerial(il_serial, buf_0+2, len-2);
    ReadSerial(il_serial, buf_1, len);
    ReadSerial(il_serial, buf_2, len);

    return true;
}


int main(int argc, char **argv)
{
   /* Init Node.*/
    ros::init(argc, argv, "indor_localization_node");
    ros::NodeHandle n;

   /* Create and init the serial port for indoor-localization.*/
    //LibSerial::SerialStream il_serial("/dev/ttyUSB0",std::ios_base::in);
    LibSerial::SerialStream il_serial("/dev/pts/23",std::ios_base::in);
    InitSerialPort(il_serial);
    tf::TransformBroadcaster tf_pub;

    while(ros::ok()){
        /** Receive data from serial port. */
        const int BUFFER_SIZE = 11 ;
        char buf_0[BUFFER_SIZE];
        char buf_1[BUFFER_SIZE];
        char buf_2[BUFFER_SIZE];
        if(ReadPosSensor(il_serial, buf_0, buf_1, buf_2, BUFFER_SIZE)){
            int x, y;
            // computing position
            if(!compute_pos(buf_0, buf_1, buf_2, x, y)){
                std::cerr<<"Error: "<<std::endl;
            }

            // publish to tf
            tf_pub.sendTransform(
                tf::StampedTransform(
                    tf::Transform(tf::Quaternion(0,0,0,1),tf::Vector3(x,y,0)),
                    ros::Time::now(),
                    "base",
                    "robot"));
        }

    }

    il_serial.Close() ;
    
    return 0;
}


